/*
 * LocalizationFilter.cpp
 *
 *  Created on: 06.12.2011
 *      Author: jan
 */

#include "LocalizationFilter.h"

namespace std {

LocalizationFilter::~LocalizationFilter() {
}

void LocalizationFilter::setHeading(double h){
	heading = h;
}

void LocalizationFilter::create(){
	vector<Particle<DoubleProb> > * parts = &particles;
	Particle<DoubleProb>::create_dummy(parts, nmbr, m);
	cout << "LocalizationFilter: got Particles" << endl;
	new_particles.reserve(nmbr);
}
void LocalizationFilter::predict(double head,double speed){
	double delta_heading = heading - head;
	for(unsigned int i = 0; i < particles.size(); i++){
		particles[i].heading += delta_heading;
		particles[i].advance_by(speed,m,resol);
	}
}
void LocalizationFilter::normalize(){
	norm=abs(norm);
	for(unsigned int i = 0; i < particles.size(); i++){
		particles[i].w /= norm;
//		cout << "["<< particles[i].p.x << ":" << particles[i].p.y  << "]("<< particles[i].w <<") ";
	}
//	cout << endl;
}
void LocalizationFilter::weight(){
	if(dist != NULL) delete dist;
//	cout << " WD created "<<endl;
	dist = new WeightedDistribution<DoubleProb>(particles);
}

void LocalizationFilter::resample(){
	weight();
//	cout << "distri created"<<endl;
	Particle<DoubleProb> p;
//	int cnt = 0;
//	for(unsigned int i = 0; i < m->data.size(); i++){
//		for(unsigned int j = 0; j < m->data[0].size(); j++){
//			if(m->data[i][j].getValue()  != (double)-1){
//				cnt++;
//				cout << " ["<<(i)<<":"<<(j)<<"]("<<m->data[i][j].getValue()<<")";
//			}
//		}cout << endl;
//	}
//	cout <<"count: "<< cnt<<endl;

	for(unsigned int i = 0; i < particles.size(); i++){
		p = dist->pick();
		if(p.x == -1 && p.y == -1){
//			cout << "now creating one..."<<endl;
			Particle<DoubleProb>::create_random(&new_particles, 1, m);
			new_particles.back().heading = heading;
		}else{
			p.w = particleweights;
			p.heading = heading;
			new_particles.push_back(p);
		}
//		cout << "next particle..."<< i <<endl;
	}
//	cout << "finished with particles.." << endl;
	particles.clear();
	particles.insert(particles.begin(),new_particles.begin(),new_particles.end());
	new_particles.clear();
}

double LocalizationFilter::bresenham(Point p, Point t, bool wall) {
	int penalty = -10;
	int startX = static_cast<int>(t.x);
	int startY = static_cast<int>(t.y);
	int endX = static_cast<int>(p.x);
	int endY = static_cast<int>(p.y);
	int dx = abs(endX - startX);
	int sx = startX < endX ? 1 : -1;
	int dy = -abs(endY - startY);
	int sy = startY < endY ? 1 : -1;
	int x = startX, y = startY;
	int err = dx + dy;
	int e2 = 0;
	double w = 0;
	for (int i = 0; i < 3; i++) {
		if (wall && i == 3){
			if ((!(m->is_free(Point(x, y))))) { // wall ist besetzt
				w += (m->data[x][y].getValue());
			} else {
				w += penalty;
			}
		} else {
			if (m->is_free(Point(x, y))) {
				w += (1 - m->data[x][y].getValue()); // freies feld
			} else {
				w += penalty;
			}
		}
		if (i == 1) {
			err = dx + dy;
			x = startX;
			y = startY;
			sx = -sx;
			sy = -sy;
		}
		e2 = err * 2;
		if (e2 > dy) {
			err += dy;
			x += sx;
		}
		if (e2 < dx) {
			err += dx;
			y += sy;
		}
	}

	return w;
}

#define SIGMAVAL
#define BRESENHAM

void LocalizationFilter::measure(Measurement m){
	this->norm = 0;
	int resol = (int) (this->resol);
	int counter = 0;
	unsigned int pointcounter = 0;
	for(unsigned int i = 0; i < particles.size(); i++){
		if(this->m->is_free(particles[i].p)){
			double w = 0;
			Point p;
			counter = 0;
			pointcounter = 0;
			double value = 0;
#ifdef SIGMAVAL
			for(unsigned int j = 0; j < m.points.size(); j++){
				p.x = (m.points[j].x*resol+particles[i].p.x);
				p.y = (m.points[j].y*resol+particles[i].p.y);
				if(this->m->is_in(p)){
					value += (this->m->data[(int)p.x][(int)p.y].getValue());
				}
			}
			w = Particle<DoubleProb>::w_gauss(this->m->mapvalue,value,resol);
#else
#ifdef BRESENHAM
			for(unsigned int j = 0; j < m.points.size(); j++){
				p.x = particles[i].p.x+m.points[j].x*resol;
				p.y = particles[i].p.y+m.points[j].y*resol;
				value+=bresenham(particles[i].p,p,m.wall[j]);
			}
			w = Particle<DoubleProb>::w_gauss(this->m->mapvalue,value,resol);
#else





			for(int j = 0, degree = -90; degree <= 90; degree+=m.grad,j++){
				double rad = 0;
				double last = 5;
				for(double radius = m.radius; radius > 0; radius-=m.divlen){
					double xk = radius * cos((particles[i].heading+degree)/180*M_PI); // d / 360 = r / 2*PI
					double yk = radius * sin((particles[i].heading+degree)/180*M_PI); // r = d*2PI/360 = d*PI/180
					if((int)(m.points[j].x*resol) == (int)(xk*resol) && (int)(m.points[j].y*resol) == (int)(yk*resol)) pointcounter++;
					Point l((particles[i].p.x/resol+xk)*resol,(particles[i].p.y/resol+yk)*resol);
					if(this->m->is_free(l)){
						rad = radius;
						p.x = l.x; p.y = l.y;
						last = radius;
						radius = 0; // for loop breaken
					}

				}
				if(m.scan[j] == rad){
					counter++;
					w += (this->m->data[(int)(p.x)][(int)(p.y)].getValue());
				}else{
					if((this->m->data[(int)(p.x)][(int)(p.y)].getValue()) != -1){
						w += -10;
					}
				}
			}
#endif
#endif
			particles[i].w = w;
			if( w > 0 ) norm +=w;
		}else{
			particles[i].w = 0;
		}
	}
	normalize();
}

} /* namespace std */
